#include <Servo.h>
#include <time.h>
using namespace std;
Servo servos[4];
double servooo[4]={511,511,511,511}; //servo angle from 0 to 1
int tiltPT_old=0; //no chyba zero, co?
int tiltPL_old=0; //j.w.
const double kp=3;
const double kd=10;
const double ki=10;
int I_T=0;
int I_L=0;
double D[2];
int val=-1;

time_t time_old;

void setup(){
  for(int i=0; i<4; i++){
    servos[i].attach(i+2); //serwa są na pinach od 2 do 7
    servos[i].write(90);
  }
  pinMode(A0,INPUT); //tilt Przód-Tył
  pinMode(A1,INPUT); //tilt Prawo-Lewo
  tiltPT_old=analogRead(A0)/4095.0 - 0.5;
  tiltPL_old=analogRead(A1)/4095.0 - 0.5;
}

void loop(){
  time_old=time(NULL);
  for(int i=0;i<4;i++){
  	val = map(int(servooo[i]), 0, 1023, 0, 180);
    servos[i].write(val);
  }
  int tilt_PT=analogRead(A0) - 512; //pomiar przechyłu przód-tył
  int tilt_PL=analogRead(A1) - 512; //pomiar przechyłu prawo-lewo
  D[1]=(tilt_PT-tiltPT_old)/difftime(time(NULL),time_old);
  D[2]=(tilt_PL-tiltPL_old)/difftime(time(NULL),time_old);
  I_T += tilt_PT*difftime(time(NULL),time_old);
  I_L += tilt_PL*difftime(time(NULL),time_old);
  servooo[1]=kp*(tilt_PT+tilt_PL)+kd*(D[1]+D[2])+ki*(I_T+I_L)+0.5;
  servooo[2]=kp*(0-tilt_PT+tilt_PL)+kd*(0-D[1]+D[2])+ki*(0-I_T+I_L)+0.5;
  servooo[3]=kp*(0-tilt_PT-tilt_PL)+kd*(0-D[1]-D[2])+ki*(0-I_T-I_L)+0.5;
  servooo[4]=kp*(tilt_PT-tilt_PL)+kd*(D[1]-D[2])+ki*(I_T-I_L)+0.5;
  for(int i=0; i<4; i++){
    if(servooo[i]<0) {
      servooo[i]=0;
    }
    if(servooo[i]>1023) {
      servooo[i]=1023;
    }
  }
  tiltPT_old=tilt_PT;
  tiltPL_old=tilt_PL;
}